#include "ofxONI.h"

ofxONI::ofxONI()
{

}

ofxONI::~ofxONI()
{
	g_Context.Shutdown();
}

void ofxONI::setup()
{
	/* All of this was here before, nothing below in the setup function is my code */

	XnStatus nRetVal = XN_STATUS_OK;
	bDrawPlayers = true;
	bDrawCam = true;

	printf("InitFromXmlFile\n");
	nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH);
	CHECK_RC(nRetVal, "InitFromXml");
	
	printf("FindExistingNode\n");
	nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
	CHECK_RC(nRetVal, "Find depth generator");
	nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
	if (nRetVal != XN_STATUS_OK)
	{
		nRetVal = g_UserGenerator.Create(g_Context);
		CHECK_RC(nRetVal, "Find user generator");
	}

	printf("FindExistingNode\n");
	nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
	CHECK_RC(nRetVal, "Find image generator");

	nRetVal = g_Context.StartGeneratingAll();
	CHECK_RC(nRetVal, "StartGenerating");
	
	XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
	if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
	{
		printf("Supplied user generator doesn't support skeleton\n");
	}

	g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
	g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);

	if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
	{
		g_bNeedPose = TRUE;
		if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
		{
			printf("Pose required, but not supported\n");
		}
		g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
		g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
	}

	g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

	ofBackground(255,255,255);
		
	ofSetVerticalSync(true);

    //some model / light stuff
    glEnable (GL_DEPTH_TEST);
    glShadeModel (GL_SMOOTH);

    /* initialize lighting */
    glLightfv (GL_LIGHT0, GL_POSITION, lightOnePosition);
    glLightfv (GL_LIGHT0, GL_DIFFUSE, lightOneColor);
    glEnable (GL_LIGHT0);
    glLightfv (GL_LIGHT1, GL_POSITION, lightTwoPosition);
    glLightfv (GL_LIGHT1, GL_DIFFUSE, lightTwoColor);
    glEnable (GL_LIGHT1);
    glEnable (GL_LIGHTING);
    glColorMaterial (GL_FRONT_AND_BACK, GL_DIFFUSE);
    glEnable (GL_COLOR_MATERIAL);

    //load the squirrel model - the 3ds and the texture file need to be in the same folder
    squirrelModel.loadModel("squirrel/NewSquirrel.3ds", 20);

    //you can create as many rotations as you want
    //choose which axis you want it to effect
    //you can update these rotations later on
    squirrelModel.setRotation(1, 270, 1, 0, 0);
    squirrelModel.setRotation(1, 270, 0, 0, 1);
	squirrelModel.setRotation(1, 270, 0, 1, 0);
    squirrelModel.setScale(0.9, 0.9, 0.9);
    squirrelModel.setPosition(ofGetWidth()/2, ofGetHeight()/2, 0);
}


void ofxONI::update()
{		
	g_Context.WaitAndUpdateAll();
	PrintSessionState(g_SessionState);
}

// DRAW SKELETON
void ofxONI::drawSkeletonPt(XnUserID player, XnSkeletonJoint eJoint)
{

	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	XnSkeletonJointPosition joint;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

	XnPoint3D pt;
	pt = joint.position;
	float ptz = pt.Z;

	float radZ = 25 - ptz/100;
	if(radZ < 3) radZ=3;

	g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);

	if (eJoint == XN_SKEL_LEFT_HAND){
		LHandPoint = pt;
		float handsDistance = abs(LHandPoint.X - LastRHandPoint.X);
		float gripperOpenAngle = handsDistance / 2.78;
		float avgHandsHeight = (LHandPoint.Y + LastRHandPoint.Y) / 2;
		float elbowAngle = avgHandsHeight / 1.87;
		//printf("distance: %f Angle: %f height: %f angle:%f  \n",handsDistance, gripperOpenAngle, avgHandsHeight,elbowAngle);
		x = gripperOpenAngle;
	}
	LastLHandPoint=LHandPoint;

	if (eJoint == XN_SKEL_RIGHT_HAND){
		RHandPoint = pt;
		float handsDistance = abs(RHandPoint.X - LastLHandPoint.X);
		float gripperOpenAngle = handsDistance / 2.78;
		float avgHandsHeight = (RHandPoint.Y + LastLHandPoint.Y) / 2;
		float elbowAngle = avgHandsHeight / 1.87;
		//printf("distance: %f Angle: %f height: %f angle:%f  \n",handsDistance, gripperOpenAngle, avgHandsHeight,elbowAngle);
		x = gripperOpenAngle;
		LastRHandPoint=RHandPoint;	
	}
}

void ofxONI::skeletonTracking()
{
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;

	g_UserGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{
		if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
		{
			drawSkeletonPt(aUsers[i], XN_SKEL_LEFT_HAND);
			drawSkeletonPt(aUsers[i], XN_SKEL_RIGHT_HAND);
		}
	}

		 //fake back wall
    ofSetColor(20, 20, 20);
    glBegin(GL_QUADS);
        glVertex3f(0.0, ofGetHeight(), -600);
        glVertex3f(ofGetWidth(), ofGetHeight(), -600);
        glVertex3f(ofGetWidth(), 0, -600);
        glVertex3f(0, 0, -600);
    glEnd();

    //fake wall
    ofSetColor(50, 50, 50);
    glBegin(GL_QUADS);
        glVertex3f(0.0, ofGetHeight(), 0);
        glVertex3f(ofGetWidth(), ofGetHeight(), 0);
        glVertex3f(ofGetWidth(), ofGetHeight(), -600);
        glVertex3f(0, ofGetHeight(), -600);
    glEnd();

    //lets tumble the world with the mouse
    glPushMatrix();

        //draw in middle of the screen
        glTranslatef(ofGetWidth()/2,ofGetHeight()/2,0);
        
		//tumble according to mouse
		glRotatef(LHandPoint.Y,1,0,0);
		glRotatef(LHandPoint.X,0,1,0);
		glRotatef(RHandPoint.Z,0,0,1);
        glTranslatef(-ofGetWidth()/2,-ofGetHeight()/2,0);

        ofSetColor(255, 255, 255, 255);
        squirrelModel.draw();

    glPopMatrix();
	
	ofSetColor(0x000000);
	ofDrawBitmapString("Current Status: "+ status, 10, 15);
}
